
#include<tool/libcell_tools_algorithm.h>
#include<Mesh/Mesh_Operator.h>
#include <tool/libcell_kd_node_iterator.h>
#include<tool/libcell_octree_iterator.h>
#include<Algorithm/points_cloud_algorithm.h>
#include <Dense>
#include<SVD>
using namespace Eigen;



#include "pre_handle.h"
#include<time.h>
#define quote lib_cell_quote


static inline double compute_visual_avarage_distance(Node* n)
{
    double* box=mesh_compute_bounding_box_from_node(n);
    double min1=box[3]-box[0],min2=box[4]-box[1],min3=box[5]-box[2];
    double mid=min1>=min2?(min3>=min1?(min1):(min2>=min3?(min2):(min3))):(min3>=min2?(min2):(min1>=min3?(min1):(min3)));
    double vdis=mid/70.0;
    return vdis;
}

double get_fac_from_model(double** model,int len )
{
    //double * box=mesh_compute_bounding_box(model);
    double box[6] = {model[0][0],model[0][1],model[0][2],model[0][0],model[0][1],model[0][2] };
    for (int i=0;i<len;i++)
    {
        for (int j=0;j<3;j++)
        {
            if(model[i][j]<box[j])
            {
                box[j] = model[i][j];
            }
            if (model[i][j] > box[j+3])
            {
                box[j+3] = model[i][j];
            }
        }
    }   

    double bound[3]={box[3]-box[0],box[4]-box[1] ,box[5]-box[2]};
    for(int i=0;i<2;i++)
    {
        for(int j=0;j<2;j++)
        {
            if(bound[j]<bound[j+1])
            {
                double temp=bound[j];
                bound[j]=bound[j+1];
                bound[j+1]=temp;
            }
        }
    }
    double re= 0.192/bound[0];
    printf("bound :%lf %lf %lf\n",bound[0],bound[1],bound[2]);
    //free(box);
    return re;
}
OC_Node* get_oc_node_according_to_size(Mesh* m, double len)
{
    OC_Node* ocn = (OC_Node*)malloc(sizeof(OC_Node));
    oc_node_init(ocn);

    auto vit = m->v_begin(m);
    template_v* v = quote(vit);

    ocn->loc_min[0] = v->point[0];  ocn->loc_min[1] = v->point[1]; ocn->loc_min[2] = v->point[2];
    ocn->loc_max[0] = v->point[0];  ocn->loc_max[1] = v->point[1]; ocn->loc_max[2] = v->point[2];

    for (; vit != m->v_end(m); vit++)
    {
        v = quote(vit);
        ocn->value = node_overlying(ocn->value, v);
        for (int i = 0; i < 3; i++)
        {
            v->point[i] > ocn->loc_max[i] ? (ocn->loc_max[i] = v->point[i]) : (v->point[i] < ocn->loc_min[i] ? (ocn->loc_min[i] = v->point[i]) : (i = i));
        }
    }
    double bound[3] = { ocn->loc_max[0] - ocn->loc_min[0],ocn->loc_max[1] - ocn->loc_min[1],ocn->loc_max[2] - ocn->loc_min[2] };
    for (int i = 0; i < 2; i++)
    {
        for (int j = 0; j < 2; j++)
        {
            if (bound[j] < bound[j + 1])
            {
                double temp = bound[j];
                bound[j] = bound[j + 1];
                bound[j + 1] = temp;
            }
        }
    }
    int k = 0; double temp_len = len;
    for (int i = 0; i < 90; i++)
    {
        temp_len = temp_len * 2;
        k = i + 1;
        if (temp_len > bound[0])
        {
            break;
        }
    }
    printf("k:%d %lf %lf\n", k, bound[0], temp_len);


    ocn->loc_max[0] = ocn->loc_min[0] + temp_len; ocn->loc_max[1] = ocn->loc_min[1] + temp_len; ocn->loc_max[2] = ocn->loc_min[2] + temp_len;


    for (int i = 0; i < k; i++)
    {

        oc_node_divide_all_leaves(ocn);
    }
    return ocn;
}


Mesh* downsampling_points(Mesh* m, double len)
{
    OC_Node* ocn = get_oc_node_according_to_size(m, len);
    Mesh* re = (Mesh*)malloc(sizeof(Mesh));
    Mesh_init(re);
    double p[3] = { 0,0,0 };
    for (auto oit = octree_begin(ocn); oit != octree_end(ocn); oit++)
    {
        if (oit.it->value != NULL)
        {
            memset(p, 0, sizeof(double) * 3);
            double sum = 0;
            for (Node* nit = oit.it->value; nit != NULL; nit = (Node*)(nit->Next))
            {
                template_v* v1 = (template_v*)(nit->value);
                p[0] += v1->point[0]; p[1] += v1->point[1]; p[2] += v1->point[2];

                sum += 1;
            }
            p[0] /= sum; p[1] /= sum; p[2] /= sum;
            re->create_vertexv(re, p, 3);
        }
    }
    oc_node_free(ocn);
    return re;
}


void my_adjust_normal(Node* points_n,KD_Node*kdtree ,int is_inverse)
{
    // mesh_adjust_mesh_to_unit_hexahedron(m);
    // KD_Node* kdtree = create_kd_tree_from_mesh(m);

    // Node* points_n = NULL;
    // for (auto it = m->v_begin(m); it != m->v_end(m); it++)
    // {
    //     points_n = node_overlying(points_n, quote(it));
    // }
    double r = compute_visual_avarage_distance(points_n);
    //printf("dadsar:%lf\n", r);
    adjust_normal_of_node_vertices(points_n, 6* r, kdtree);//6*r

    //printf("here compute\n");
    //int i = 0;
    for (Node* nit=points_n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        double* normal = (double*)(v->prop);

        if (is_inverse)
        {
            normal[0]=-normal[0];normal[1]=-normal[1];normal[2]=-normal[2];
            //cloud->points[i].normal_x = -normal[0]; cloud->points[i].normal_y = -normal[1]; cloud->points[i].normal_z = -normal[2];
        }
        //else
        //{
            //cloud->points[i].normal_x = normal[0]; cloud->points[i].normal_y = normal[1]; cloud->points[i].normal_z = normal[2];
        //}
        //cloud->points[i].x=quote(it)->point[0];cloud->points[i].y=quote(it)->point[1];cloud->points[i].z=quote(it)->point[2];

        //SAFE_FREE(quote(it)->prop);
        //i++;
    }
    //free_node(points_n);
    //free_kdnode(kdtree);
    //Mesh_free(&mesh);
}

double*compute_pca_result_using_eigen(Node* n)
{
    int len = node_size(n);

    if(len<3)
    {
        return NULL;
    }
    template_v* v=(template_v*)(n->value);

    Eigen::MatrixXd A;
    A.resize(len,v->point_size);
    A.setZero();

    int i=0;
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        v=(template_v*)(nit->value); 
        for(int j=0;j<v->point_size;j++)
        {
            A(i,j)=v->point[j];
        }
        i++;
    }
    Eigen::MatrixXd B=(MatrixXd::Ones(1,len)*A)*((1.0/(double)len));
    for(int i=0;i<len;i++)
    {
        A.row(i)=A.row(i)-B;
    }
    JacobiSVD<Eigen::MatrixXd> svd(A,ComputeThinU|ComputeThinV);
    MatrixXd V=svd.matrixV(); 
    //printf("%lf %lf\n",V(0,0),V(0,1));
    //printf("%d %d\n",V.rows(),V.cols());

    double* re=(double*)malloc(sizeof(double)*9); 
    memmove(re,V.data(),sizeof(double)*9);
    return re;
}


void points_cloud_compute_normal(Node* n,KD_Node*kdtree,double r )
{
    MatrixXd A(3,3);
    clock_t t = clock();
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        double * normal=(double*)malloc(sizeof(double)*3);
        
        template_v* v=(template_v*)(nit->value);
        v->prop=normal;

        Node* n1=kd_tree_find_nearest_sphere(v,r,kdtree);
        //Node* n1=kd_tree_find_n_adjacent_verticesn(v,kdtree,8);

        int num =node_size(n1); 

        double* axis=compute_pca_result_using_eigen(n1);
        if (axis == NULL)
        {
            normal[0] = 0; normal[1] = 0; normal[2] = 1;
            free_node(n1);
            
            continue;
        }

        normal[0]=axis[6];normal[1]=axis[7];normal[2]=axis[8];
        normalize(normal,3);
        free(axis);
    }
    printf("运行时间%.5f秒:%d\n", (double)(clock() - t) / CLOCKS_PER_SEC,0); 
}